package com.example.auv_660.autocontrolalgorithm;

import com.example.auv_660.MainActivity;
import com.example.auv_660.autocontrolalgorithm.assistantcalculator.AutoControl;
import com.example.auv_660.autocontrolalgorithm.assistantsensor.BatterySensor;
import com.example.auv_660.autocontrolalgorithm.assistantsensor.LocationSensor;
import com.example.auv_660.autocontrolalgorithm.assistantsensor.OrientationSensor;
import com.example.auv_660.command.AUVBatteryCommand;
import com.example.auv_660.command.AUVDepthSensorCommand;
import com.example.auv_660.command.AUVPropeller;
import com.example.auv_660.command.AUVPropeller.AUVPropellerCommand;
import com.example.auv_660.network.udp.HFUDPBuild;
import com.example.auv_660.util.Constants;
import com.example.auv_660.util.FileUtils;
import com.example.auv_660.util.MsgType;
import com.example.auv_660.util.TaskIUtils;
import com.example.auv_660.util.ThreadPool;

import java.io.BufferedReader;
import java.io.IOException;
import java.net.InetAddress;
import java.util.ArrayList;

public class AutoControlTypel extends Strategy {
    private double duration = 10 * 1000;
//    private static final String RESET_ORDER = "7E08701414141432FA";
    private double rotate;
    private double setDepth = -10;
    private double refYaw = 0.0;
    private int warnOri = 0;
    private OrientationSensor orientationSensor;
    private AUVPropellerCommand auvPropellerCommand;
    private AUVDepthSensorCommand auvDepthSensorCommand;
    private AUVBatteryCommand auvBatteryCommand;
    private BatterySensor batterySensor;
    private BufferedReader bufferedReader;
    private LocationSensor locationSensor;
    private MainActivity mainActivity = MainActivity.getInstance();

    private int DOWNTIME = 10000;

    @Override
    public void doOperation(String order) {
        String[] parameters = order.split(",");

        if (parameters.length == 9)
        {
            duration = Double.valueOf(parameters[1]) * 1000;
            rotate = Double.valueOf(parameters[2]);
            setDepth = Double.valueOf(parameters[3]);
            mainActivity.sendMsgToMainThread(MsgType.CONTROLLER_MSG, "直航");

            Constants.S1 = Double.valueOf(parameters[4]);
            Constants.D1 = Double.valueOf(parameters[5]);
            Constants.D2 = Double.valueOf(parameters[6]);
            Constants.RDDPARA = Double.valueOf(parameters[7]);
            Constants.chadong = Double.valueOf(parameters[8]);
        }
        else
        {
            return;
        }

        orientationSensor = new OrientationSensor();

        float[] checkValue = orientationSensor.getValue();

        while (checkValue[0] == 0.0 && checkValue[1] == 0.0 && checkValue[2] == 0.0)
        {
            checkValue = orientationSensor.getValue();
        }

        refYaw = orientationSensor.getValue()[0];
        System.out.println("refYaw: " + refYaw);

        System.out.println("duration: " + duration);
        System.out.println("rotate: " + rotate);
        System.out.println("setDepth: " + setDepth);

        locationSensor = new LocationSensor();
        auvPropellerCommand = AUVPropeller.INSTANCE.getAuvPropellerCommand();
        auvDepthSensorCommand = new AUVDepthSensorCommand();
        auvBatteryCommand = new AUVBatteryCommand();
        batterySensor = new BatterySensor();

        Constants.ORDER = false;

        autoControlInFixTime();

        System.out.println("autoControlFixTime Done!!");
    }

    private void autoControlInFixTime()
    {
        ThreadPool.INSTANCE.getThreadPoolProxy().joinTask(new Runnable() {
            @Override
            public void run() {
                Constants.RUN = true;

                FileUtils.INSTANCE.getFileWrite().writeDSDATA("" + "Yaw" + " " + "Pitch" + " " + "Roll"
                        +  " " + "Depth" + " " + "TopRudder" + " "  + "BottomRudder" + " " + "LeftRudder"
                        + " " + "RightRudder" + " " + "Rotation" + " " + "Chadong" + " " + "AUVBattery"
                        + " " + "longtitude" + " "+ "latitude" + "\n");

                int downTime;
                long downBegin = System.currentTimeMillis();

                if (duration > DOWNTIME)
                {
                    downTime = 100;
                }
                else
                {
                    downTime = (int) duration / 100;
                }

                for (int i = 0; i < downTime; i++)
                {
                    auvPropellerCommand.writeCommand(Constants.AUV_PROPELLER_ORDER_PREFIX + Constants.AUV_DOWN_WATER + Constants.AUV_ORDER_SUFFIX);
                    try
                    {
                        Thread.sleep(100);
                        float[] values = orientationSensor.getValue();
                        double curDepthAC = auvDepthSensorCommand.getDepthAC();
                        String battery = auvBatteryCommand.getBatteryValue();

                        if (battery.contains("无"))
                        {
                            battery = "0";
                        }
                        else
                        {
                            battery = battery.split("\\%")[0];
                        }

                        double[] location = locationSensor.getValue("SCREEN");

                        FileUtils.INSTANCE.getFileWrite().writeDSDATA("" + values[0] + " " + values[1] + " "
                                + values[2] +  " " + curDepthAC + " " + "0" + " "  + "0" + " " + "15" + " "
                                + "-20" + " " + "3000" +  " " + "0" + " " + battery + " " + location[0] + " "
                                + location[1] + "\n");
                    }
                    catch (InterruptedException e)
                    {
                        e.printStackTrace();
                    }
                }

                System.out.println("down time: " + (System.currentTimeMillis() - downBegin) / 1000);

                long begin = System.currentTimeMillis();

                while (Constants.RUN)
                {
                    try
                    {
                        long start = System.currentTimeMillis();
                        startAutoControl();
                        long mSleep = System.currentTimeMillis() - start;
                        if (mSleep < 100)
                        {
                            Thread.sleep(100 - mSleep);
                        }
                    }
                    catch (InterruptedException e)
                    {
                        e.printStackTrace();
                    }

                    long checkTime = System.currentTimeMillis();
                    long time = checkTime - begin;
                    System.out.println("autoControlInFixTime time: " + time + " duration: " + (duration));
                    if (time > duration - DOWNTIME)
                    {
                        Constants.ORDER = true;
                        orientationSensor.unregister();
                        FileUtils.INSTANCE.getFileWrite().realseWriteDSData();

                        try
                        {
                            Thread.sleep(100);
                            auvPropellerCommand.writeCommand(Constants.AUV_PROPELLER_ORDER_PREFIX + Constants.AUV_SELF_TEST_PROPELLER_ORDER_II
                                    + Constants.AUV_ORDER_SUFFIX);
                        }
                        catch (InterruptedException e)
                        {
                            e.printStackTrace();
                        }
                        break;
                    }
                }

                orientationSensor.unregister();
                FileUtils.INSTANCE.getFileWrite().realseWriteDSData();

                boolean linkCheck = false;
                while (!linkCheck)
                {
                    try
                    {
                        linkCheck = InetAddress.getByName(Constants.SERVER_IP).isReachable(10000);
                    }
                    catch (IOException e)
                    {
                        e.printStackTrace();
                    }
                }

                HFUDPBuild.INSTANCE.getHFUdpClient().sendMessage("CENDTIME" + Constants.PHONEIMEI);
                Constants.ORDER = true;
            }
        });
    }

    private void startAutoControl()
    {
        String tailOrder = "";
        float[] values = orientationSensor.getValue();

        double curDepthAC = auvDepthSensorCommand.getDepthAC();
        String battery = auvBatteryCommand.getBatteryValue();
        double[] location = locationSensor.getValue("SCREEN");

        if (battery.contains("无"))
        {
            battery = "0";
        }
        else
        {
            battery = battery.split("\\%")[0];
        }

        double[] tailCtrl = AutoControl.tailSet(values[0], values[1], values[2], refYaw, curDepthAC, setDepth, rotate);

        if (tailCtrl == null)
        {
            return;
        }

        int length = tailCtrl.length;
        for (int i = 0; i < length - 2; i++)
        {
            tailOrder += (int) tailCtrl[i] + ",";
        }
        tailOrder += (int) tailCtrl[length - 2];

        String orderControl = Constants.AUV_PROPELLER_ORDER_PREFIX + tailOrder + Constants.AUV_ORDER_SUFFIX;        // TODO 注意协议格式

        System.out.println("orderControl: " + orderControl);

        FileUtils.INSTANCE.getFileWrite().writeDSDATA("" + values[0] + " " + values[1] + " " + values[2] +  " " + curDepthAC
                + " " + tailCtrl[0] + " "  + tailCtrl[1] + " " + tailCtrl[2] + " " + tailCtrl[3] + " " + tailCtrl[4] + " "
                + tailCtrl[5] + " " + battery + " " + location[0] + " " + location[1] + "\n");

        auvPropellerCommand.writeCommand(orderControl);
    }

    private ArrayList<String[]> readMissionFile()
    {
        TaskIUtils taskIUtils = new TaskIUtils();
        bufferedReader = taskIUtils.readTask();

        ArrayList<String[]> mission = new ArrayList<>();

        if (bufferedReader == null)
        {
            System.out.println("没有找到TASKINFORMATION文件");
        }
        else
        {
            String line;

            try
            {
                line = bufferedReader.readLine();

                while (null != line)
                {
                    String[] information = line.split("\n")[0].split(",");
                    String[] content = new String[6];

                    for (int i = 0; i < information.length; i++)
                    {
                        content[i] = information[i];
                    }
                    mission.add(content);

                    line = bufferedReader.readLine();
                }
            }
            catch (IOException e)
            {
                taskIUtils.realseReadTask();
            }
        }
        taskIUtils.realseReadTask();
        return mission;
    }

    private Boolean checkWarning()
    {
        float[] orientation = orientationSensor.getValue();

        if (orientation[0] == 0.0 && orientation[1] == 0.0 && orientation[2] == 0.0)
        {
            warnOri ++;
        }
        else
        {
            warnOri = 0;
        }

        if (warnOri == 50)
        {
            System.out.println("WARN: 1");
            Constants.RUN = false;
            return true;
        }

        double curDepthAC = auvDepthSensorCommand.getDepthAC();
        if (curDepthAC > Constants.CANNOTDEPTH)
        {
            System.out.println("WARN: 2");
            if (mainActivity != null && curDepthAC != 1000)
            {
                mainActivity.sendMsgToMainThread(MsgType.RED_CHAO_SHEN, "");
            }
            Constants.DHWARNNING = 1;
            Constants.RUN = false;
            return true;
        }

        String tmp = auvBatteryCommand.getBatteryValue();
        int auvBattery;
        if (tmp.equals("暂无电量显示"))
        {
            auvBattery = 0;
        }
        else
        {
            auvBattery = Integer.parseInt(tmp.split("%")[0]);
        }

        int phoneBattery = batterySensor.getCurrentBattery();

        if (auvBattery <= 20 || phoneBattery <= 20) {
            System.out.println("WARN: 3");
            Constants.RUN = false;
            Constants.DHWARNNING = 3;
            return true;
        }
        return false;
    }
}
